﻿using System;
using System.Collections.Generic;
using System.Text;
using System.Drawing;

namespace MonoPhysics
{
    public class Quadtree<T>
    {
        public Quadtree<T> f1;
        public Quadtree<T> f2;
        public Quadtree<T> f3;
        public Quadtree<T> f4;
        private T value;


        public T Key
        { get { return value; } }

        public Quadtree(T t_element)
        { this.value = t_element; }
    }

    public class BinaryTree<T>
    {
        public delegate void ForallFunc(T elt);

        public BinaryTree<T> fg;
        public BinaryTree<T> fd;
        public BinaryTree<T> pere;
        public int parseKey;
        private T value;


        public T Value
        {
            get { return value; }
            set { this.value = value; }
        }

        public BinaryTree(T value)
        { this.value = value; }

        public void Forall(BinaryTree<T> t, ForallFunc f)
        {
            if (t != null)
            {
                Forall(t.fg, f);
                Forall(t.fd, f);
                f(t.value);
            }
        }
    }

    public class Circle
    {
        public readonly RigidBody Pere;
        public readonly float radius;
        private PointD center;
        public readonly List<SegmentD> lyingSegments;
        private float orientation = 0;

        public float Orientation
        {
            get { return orientation; }
            set { this.orientation = value; }
        }
        public PointD Center
        {
            get { return center; }
            set { this.center = value; }
        }
        public Circle(PointD center, float radius, List<SegmentD> seg, RigidBody Pere)
        {
            this.Pere = Pere;
            this.center = center;
            this.radius = radius;
            this.lyingSegments = seg;
        }

        public static Circle Fusion(Circle c1, Circle c2)
        {
            if (c1 == null)
            {
                if (c2 == null)
                    return null;
                else
                    return c2;
            }
            else
            {
                if (c2 == null)
                    return c1;
                else
                {
                    PointD center = c1.center + c2.center;
                    center.X /= 2;
                    center.Y /= 2;
                    List<SegmentD> segs = new List<SegmentD>(c1.lyingSegments);
                    foreach (SegmentD s in c2.lyingSegments)
                    {
                        if (!segs.Contains(s))
                            segs.Add(s);
                    }
                    return new Circle(center, (c1.radius + c2.radius + PointD.GetDist(c1.center, c2.center)) / 2, segs, c1.Pere);
                }
            }
        }

        public bool isIntersect(Circle c)
        {
            float dx = (this.center.X + this.Pere.Pos.X) - (c.center.X + c.Pere.Pos.X);
            float dy = (this.center.Y + this.Pere.Pos.Y) - (c.center.Y + c.Pere.Pos.Y);
            float drad = this.radius + c.radius;
            return (dy * dy + dx * dx < drad * drad);
        }

        public float DistanceTo(Circle c)
        {
            return PointD.GetDist(this.center + this.Pere.Pos, c.center + c.Pere.Pos);
        }
    }

    public static class RectFExtender
    {
        public static RectangleF Fusion(this RectangleF r1, RectangleF r2)
        {
            float Minx = Math.Min(r1.Left, r2.Left);
            float Maxx = Math.Max(r1.Right, r2.Right);
            float Miny = Math.Min(r1.Top, r2.Top);
            float Maxy = Math.Max(r1.Bottom, r2.Bottom);
            return new RectangleF(Minx, Miny, Maxx - Minx, Maxy - Miny);
        }

        public static bool IntersectWith(this RectangleF r1, RectangleF r2)
        { return (r1.X < r2.Right) && (r2.X < r1.Right) && (r1.Y < r2.Bottom) && (r2.Y < r1.Bottom); }

        public static PointF GetCenter(this RectangleF r)
        {
            return new PointF(r.Right / 2, r.Bottom / 2);
        }
    }

    public class SphereTree
    {
        private BinaryTree<Circle> hierarchy;
        public BinaryTree<Circle> Tree
        { get { return this.hierarchy; } }

        public SphereTree(Polygon p, float min, RigidBody pere)
        {
            RectangleF AABB = new RectangleF((float)(p.BoundaryRect.min[0].value), (float)(p.BoundaryRect.min[1].value), (float)(p.BoundaryRect.max[0].value - p.BoundaryRect.min[0].value), (float)(p.BoundaryRect.max[1].value - p.BoundaryRect.min[1].value));
            CircularLinkedListNode<PointD, RigidBody> pts = p.Points;
            CircularLinkedListNode<PointD, RigidBody> parc = pts;
            List<SegmentD> segs = new List<SegmentD>();
            do
            {
                segs.Add(new SegmentD(parc, parc.nodes[1].fils));
                parc = parc.nodes[1].fils;
            }
            while (parc != pts);
            Quadtree<Circle> qt;
            try
            {
                BuildQuadTree(out qt, AABB, segs, min, pere);
                QuadtreeToBinaryTree(qt, out this.hierarchy);
                SimplifyBinaryTree(ref this.hierarchy, null);
            }
            catch { }
        }

        private void BuildQuadTree(out Quadtree<Circle> q, RectangleF r, List<SegmentD> lyingSegments, float min, RigidBody pere)
        {
            if (r.Width < min && r.Height < min)// && lyingSegments.Count <= 5)
                q = null;
            else
            {
                List<SegmentD> segs = GetIntersectedLines(r, lyingSegments);
                int l = segs.Count;
                if (l > 0)
                {
                    q = new Quadtree<Circle>(new Circle(new PointD(r.X + r.Width / 2, r.Y + r.Height / 2), ((float)Math.Sqrt(r.Width * r.Width + r.Height * r.Height)) / 2, segs, pere));
                    RectangleF[] f = new RectangleF[4];
                    float wDiv2 = r.Width / 2;
                    float hDiv2 = r.Height / 2;
                    f[0] = new RectangleF(r.X, r.Y, wDiv2, hDiv2);
                    f[1] = new RectangleF(r.X + wDiv2, r.Y, wDiv2, hDiv2);
                    f[2] = new RectangleF(r.X, r.Y + hDiv2, wDiv2, hDiv2);
                    f[3] = new RectangleF(r.X + wDiv2, r.Y + hDiv2, wDiv2, hDiv2);
                    BuildQuadTree(out  q.f1, f[0], segs, min, pere);
                    BuildQuadTree(out  q.f2, f[1], segs, min, pere);
                    BuildQuadTree(out  q.f3, f[2], segs, min, pere);
                    BuildQuadTree(out  q.f4, f[3], segs, min, pere);
                    //}
                }
                else
                    q = null;
            }
        }

        private List<SegmentD> GetIntersectedLines(RectangleF r, List<SegmentD> seg)
        {
            CircularLinkedListNode<PointD, RigidBody> pt1 = new CircularLinkedListNode<PointD, RigidBody>();
            CircularLinkedListNode<PointD, RigidBody> pt2 = new CircularLinkedListNode<PointD, RigidBody>();
            pt1.value = new PointD(r.X, r.Y);
            pt2.value = new PointD(r.X + r.Width, r.Y);
            SegmentD up = new SegmentD(pt1, pt2);
            pt1.value = new PointD(r.X, r.Y + r.Height);
            pt2.value = new PointD(r.X + r.Width, r.Y + r.Height);
            SegmentD down = new SegmentD(pt1, pt2);
            pt1.value = new PointD(r.X + r.Width, r.Y);
            pt2.value = new PointD(r.X + r.Width, r.Y + r.Height);
            SegmentD right = new SegmentD(pt1, pt2);
            pt1.value = new PointD(r.X, r.Y);
            pt2.value = new PointD(r.X + r.Width, r.Y + r.Height);
            SegmentD left = new SegmentD(pt1, pt2);

            List<SegmentD> res = new List<SegmentD>();
            foreach (SegmentD s in seg)
            {
                if (GeometryHelper.ClipLine(s, r))
                    res.Add(s);
            }
            return res;
        }

        private void QuadtreeToBinaryTree(Quadtree<Circle> q, out BinaryTree<Circle> b)
        {
            // Construit l'arbre binaire de facon naive
            if (q == null)
                b = null;
            else
            {
                b = new BinaryTree<Circle>(q.Key);
                Circle nC = Circle.Fusion((q.f1 != null) ? q.f1.Key : null, (q.f2 != null) ? q.f2.Key : null);
                if (nC != null)
                {
                    b.fg = new BinaryTree<Circle>(nC);
                    QuadtreeToBinaryTree(q.f1, out b.fg.fg);
                    QuadtreeToBinaryTree(q.f2, out b.fg.fd);
                }
                else
                    b.fg = null;
                nC = Circle.Fusion((q.f3 != null) ? q.f3.Key : null, (q.f4 != null) ? q.f4.Key : null);
                if (nC != null)
                {
                    b.fd = new BinaryTree<Circle>(nC);
                    QuadtreeToBinaryTree(q.f3, out b.fd.fg);
                    QuadtreeToBinaryTree(q.f4, out b.fd.fd);
                }
                else
                    b.fd = null;
            }
        }

        private void SimplifyBinaryTree<T>(ref BinaryTree<T> c, BinaryTree<T> pere)
        {
            if (c != null)
            {
                if (c.fd != null && c.fg == null)
                {
                    c.fd.pere = c.pere;
                    c = c.fd;
                    SimplifyBinaryTree(ref c, c.pere);
                }
                else if (c.fg != null && c.fd == null)
                {
                    c.fg.pere = c.pere;
                    c = c.fg;
                    SimplifyBinaryTree(ref c, c.pere);
                }
                else
                {
                    c.pere = pere;
                    SimplifyBinaryTree(ref c.fg, c);
                    SimplifyBinaryTree(ref c.fd, c);
                }
            }
        }
    }
}